package com.pdlocation.util;

//卡尔曼滤波器
public class KalmanFilter {
    private double q = 0.00001; // 过程噪声协方差
    private double r = 0.01;    // 测量噪声协方差
    private double x = 0;       // 估计值
    private double p = 1;       // 估计误差协方差
    private double k;           // 卡尔曼增益

    public void init(double initialValue) {
        x = initialValue;
        p = 1; // 重置误差协方差
    }

    public double filter(double measurement) {
        // 预测步骤
        p = p + q;

        // 计算卡尔曼增益
        k = p / (p + r);

        // 更新估计值
        x = x + k * (measurement - x);

        // 更新估计误差协方差
        p = (1 - k) * p;

        return x;
    }

    // 动态调整测量噪声
    public void setMeasurementNoise(double noise) {
        this.r = noise;
    }

    // 动态调整过程噪声
    public void setProcessNoise(double noise) {
        this.q = noise;
    }
}
